// Copyright 2018 The Periph Authors. All rights reserved.
// Use of this source code is governed under the Apache License, Version 2.0
// that can be found in the LICENSE file.

package icm42688

import (
	"encoding/binary"
	"errors"
	"fmt"
	"sync"
	"openzbox.com/driver/conn/i2c"
	"openzbox.com/driver/conn/mmr"
	//"openzbox.com/utils"
	"openzbox.com/utils/time"
	"math"
	//"openzbox.com/driver/conn/physic"
)

// Opts holds the configuration options.
//
// Slave Address
//
// Depending which pins the A1, A0 pins are connected to will change the slave
// address. Default configuration is address 0x40 (both pins to GND). For a full
// address table see datasheet.

// PowerMonitor represents measurements from ina219 sensor.
type MotionMonitor struct {
	Magnetic D3
	Gyro D3
	Accele D3
	Temperature float64
}
type D3 struct {
	X float64
	Y float64
	Z float64
}
type AccelRange uint8

const   (
	ACCEL_RANGE_2G AccelRange = 0
	ACCEL_RANGE_4G AccelRange = 1
	ACCEL_RANGE_8G AccelRange = 2
	ACCEL_RANGE_16G AccelRange = 3
	)
type GyroRange  uint8;

const
(
	GYRO_RANGE_15_625DPS GyroRange=0
	GYRO_RANGE_31_25DPS GyroRange=1
	GYRO_RANGE_62_5DPS GyroRange=2
	GYRO_RANGE_125DPS GyroRange=3
	GYRO_RANGE_250DPS GyroRange=4
	GYRO_RANGE_500DPS GyroRange=5
	GYRO_RANGE_1000DPS GyroRange=6
	GYRO_RANGE_2000DPS GyroRange=7
)

// Registers address map
const (
	ADDRESS     byte = 0x69 //0XD0;
	ID	       byte =      0x47
	DEVICE_CONFIG = 0x11;

	PWR_RESET byte= 0x80;

	INTF_CONFIG1 byte= 0x4D;
	CLOCK_SEL_PLL byte = 0x01;

	WHO_AM_I   byte =   0X75;

	PWR_MGMT0  byte =   0x4E;
	SEN_ENABLE byte = 0x0F;

	ACCEL_CONFIG0 byte = 0x50
	ACCEL_FS_SEL_2G byte =  0x80; // TODO: 0x60 in datasheet
	ACCEL_FS_SEL_4G byte =  0x60; // TODO: 0x40 in datasheet
	ACCEL_FS_SEL_8G byte =  0x40; // TODO: 0x20 in datasheet
	ACCEL_FS_SEL_16G byte =  0x20; // TODO: 0x00 in datasheet
	ACCEL_ODR_32KHZ byte =  0x01;
	ACCEL_ODR_16KHZ byte =  0x02;
	ACCEL_ODR_8KHZ byte =  0x03;
	ACCEL_ODR_4KHZ byte =  0x04;
	ACCEL_ODR_2KHZ byte =  0x05;
	ACCEL_ODR_1KHZ byte =  0x06;
	ACCEL_ODR_200HZ byte =  0x07;
	ACCEL_ODR_100HZ byte =  0x08;
	ACCEL_ODR_50HZ byte =  0x09;
	ACCEL_ODR_25HZ byte =  0x0A;
	ACCEL_ODR_12_5HZ byte =  0x0B;
	ACCEL_ODR_6_25HZ byte =  0x0C;
	ACCEL_ODR_3_125HZ byte =  0x0D;
	ACCEL_ODR_1_5625HZ byte =  0x0E;
	ACCEL_ODR_500HZ byte =  0x0F;

	GYRO_CONFIG0 byte = 0x4F;
	GYRO_FS_SEL_15_625DPS = 0xE0;
	GYRO_FS_SEL_31_25DPS = 0xC0;
	GYRO_FS_SEL_62_5DPS = 0xA0;
	GYRO_FS_SEL_125DPS = 0x80;
	GYRO_FS_SEL_250DPS = 0x60;
	GYRO_FS_SEL_500DPS = 0x40;
	GYRO_FS_SEL_1000DPS = 0x20;
	GYRO_FS_SEL_2000DPS = 0x00;
	GYRO_ODR_32KHZ = 0x01;
	GYRO_ODR_16KHZ = 0x02;
	GYRO_ODR_8KHZ = 0x03;
	GYRO_ODR_4KHZ = 0x04;
	GYRO_ODR_2KHZ = 0x05;
	GYRO_ODR_1KHZ = 0x06;
	GYRO_ODR_200HZ = 0x07;
	GYRO_ODR_100HZ = 0x08;
	GYRO_ODR_50HZ = 0x09;
	GYRO_ODR_25HZ = 0x0A;
	GYRO_ODR_12_5HZ = 0x0B;
	GYRO_ODR_500HZ = 0x0F;

// ICM42688 registers
// BANK 0
	ACCEL_OUT byte= 0x1F;
	GYRO_OUT byte= 0x25;
	TEMP_OUT byte= 0x1D;

	BANK_SEL byte= 0x76;

    BANK0 byte= 0x00;
	BANK1 byte= 0x01;
	BANK2 byte= 0x02;
	BANK3 byte= 0x03;
	BANK4 byte= 0x04;

	// BANK 1
	GYRO_CONFIG_STATIC2 byte = 0x0B;
	GYRO_NF_ENABLE byte= 0x00;
	GYRO_NF_DISABLE byte= 0x01;
	GYRO_AAF_ENABLE byte= 0x00;
	GYRO_AAF_DISABLE byte= 0x02;

	// BANK 2
	ACCEL_CONFIG_STATIC2 byte= 0x03;
	ACCEL_AAF_ENABLE byte= 0x00;
	ACCEL_AAF_DISABLE byte= 0x01;
/*
	SENSOR_CONFIG0 byte = 0x03;
	ACCEL_DATA_X1 byte =0x1F;
	ACCEL_DATA_X0 byte =0x20;
	ACCEL_DATA_Y1 byte =0x21;
	ACCEL_DATA_Y0 byte =0x22;
	ACCEL_DATA_Z1 byte =0x23;
	ACCEL_DATA_Z0 byte =0x24;
	GYRO_DATA_X1  byte =0x25;
	GYRO_DATA_X0  byte =0x26;
	GYRO_DATA_Y1  byte =0x27;
	GYRO_DATA_Y0  byte =0x28;
	GYRO_DATA_Z1  byte =0x29;
	GYRO_DATA_Z0  byte =0x2A;
*/
)

const (
	 G = 9.807
	_d2r = 3.14159265359/180.0
  	_r2d = 180.0/3.14159265359
)

const (
	Kp =10.0                      // 这里的KpKi是用于调整加速度计修正陀螺仪的速度
	Ki= 0.008
	halfT =0.001             // 采样周期的一半，用于求解四元数微分方程时计算角增量
)
var (
	q0  = 1.0
	q1 = 0.0
	q2 = 0.0
	q3 = 0.0     // 初始姿态四元数，由上篇博文提到的变换四元数公式得来
	exInt = 0.0
	eyInt = 0.0
	ezInt = 0.0  //当前加计测得的重力加速度在三轴上的分量
)
var (
	tX  = [3]int16{0,  1,  0}
	tY  = [3]int16{1,  0,  0}
	tZ  = [3]int16{0,  0, -1}

  	_accCounts  =[3]int16{}
  	_gyroCounts = [3]int16{}

	_acc  = [3]float64{}
	_gyro = [3]float64{};

	//_accBD =[3]float32{};
	_accB= [3]float64{};
	_accS = [3]float64{1.0, 1.0, 1.0};

	_accelScale float64= 0.0;
	_accelRange AccelRange ;
	_gyroRange GyroRange;
	_gyroScale = 0.0;
	 _numSamples = 100;

	 _gyroB = [3]float64{};
	/* _tempScale float64= 333.87
	   _tempOffset float64= 21.0*/

)

type Opts struct {
	Address       int
	/*SenseResistor physic.ElectricResistance
	MaxCurrent    physic.ElectricCurrent*/
}

// DefaultOpts is the recommended default options.
var DefaultOpts = Opts{
	Address:       0x40,
/*	SenseResistor: 100 * physic.MilliOhm,
	MaxCurrent:    3200 * physic.MilliAmpere,*/
}

// New opens a handle to an ina219 sensor.
func New(bus i2c.Bus, opts *Opts) (*Dev, error) {
	i2cAddress := DefaultOpts.Address
	if opts.Address != 0 {
		if opts.Address < 0x40 || opts.Address > 0xff {
			return nil, errAddressOutOfRange
		}
		i2cAddress = opts.Address
	}
	dev := &Dev{
		m: mmr.Dev8{
			Conn:  &i2c.Dev{Bus: bus, Addr: uint16(i2cAddress)},
			Order: binary.BigEndian,
		},
	}
	return dev, nil
}

func wordToByteArray(w uint16) []byte {
	buf := make([]byte, 2)
	buf[0] = byte(w >> 8)
	buf[1] = byte(w)
	return buf
}
func (icm *Dev) Configure(confs ...uint16) error {
	// select clock source to gyro
	err :=icm.m.WriteUint8(INTF_CONFIG1,CLOCK_SEL_PLL);
	if err != nil {
		return err
	}
	// reset the ICM42688
	err =icm.m.WriteUint8(DEVICE_CONFIG,PWR_RESET);
	if err != nil {
		return err
	}
	//select clock source to gyro
	err =icm.m.WriteUint8(INTF_CONFIG1,CLOCK_SEL_PLL);
	if err != nil {
		return err
	}
	/* 读取 who am i 寄存器 */
	// check the WHO AM I byte, expected value is 0x47 (decimal 71)
	var reg_val uint8
	reg_val ,err =icm.m.ReadUint8 (WHO_AM_I);
	if err != nil {
		return err
	}
	// check the WHO AM I byte, expected value is 0x47 (decimal 71)
	if(reg_val != ID) {
		return errors.New("not found chip ICM42668.");
	}
	// enable accelerometer and gyro
	err=icm.m.WriteUint8(PWR_MGMT0,SEN_ENABLE)
	if err != nil {
		return err
	}
	// setting accel range to 16G and 32kHz as default
	//ACCEL_RANGE_2G
	//err=icm.m.WriteUint8(ACCEL_CONFIG0,ACCEL_FS_SEL_16G | ACCEL_ODR_32KHZ)
	err=icm.m.WriteUint8(ACCEL_CONFIG0,ACCEL_FS_SEL_2G | ACCEL_ODR_32KHZ)
	if err != nil {
		return err
	}
	// setting the accel scale to 16G
	_accelScale = G * 16.0/32767.5;
	_accelRange = ACCEL_RANGE_16G;

	// setting the gyro range to 2000DPS and 32kHz as default
	err=icm.m.WriteUint8(GYRO_CONFIG0,GYRO_FS_SEL_2000DPS | GYRO_ODR_32KHZ)
	if err != nil {
		return err
	}
	_gyroScale = 2000.0/32767.5* _d2r; // setting the gyro scale to 2000DPS
	_gyroRange = GYRO_RANGE_2000DPS;

	// // disable inner filters (Notch filter, Anti-alias filter, UI filter block)
	// if (setFilters(false, false) < 0) {
	//   return -7;
	// }

	// estimate gyro bias
	err = icm.calibrateGyro()
	if ( err!=nil) {
		return err;
	}

	err = icm.calibrateAccel()
	if ( err!=nil) {
		return err;
	}

	return nil
}


func  (d *Dev) readSensor() (error) {
	//_useSPIHS = true; // use the high speed SPI for data readout
	// grab the data from the ICM42688
	d.mu.Lock()
	defer d.mu.Unlock()
	var _buffer [14]byte
	err :=d.m.ReadStruct(TEMP_OUT, &_buffer)
	if ( err !=nil) {
		return err;
	}

	// combine into 16 bit values
	a1:=[2]byte{_buffer[2],_buffer[3] }
	a2:=[2]byte{_buffer[4],_buffer[5] }
	a3:=[2]byte{_buffer[6],_buffer[6] }

	g1:=[2]byte{_buffer[8],_buffer[9] }
	g2:=[2]byte{_buffer[10],_buffer[11] }
	g3:=[2]byte{_buffer[12],_buffer[13] }

	_accCounts[0] = BytesToInt16(a1[:] ) //(((int16_t)_buffer[2]) << 8) | _buffer[3];
	_accCounts[1] = BytesToInt16(a2[:] ) //(((int16_t)_buffer[4]) << 8) | _buffer[5];
	_accCounts[2] = BytesToInt16(a3[:] ) //(((int16_t)_buffer[6]) << 8) | _buffer[7];

	_gyroCounts[0] = BytesToInt16(g1[:] )
	_gyroCounts[1] = BytesToInt16(g2[:] )
	_gyroCounts[2] = BytesToInt16(g3[:] )


	_acc[0] = (float64(_accCounts[0])*_accelScale - _accB[0])*1000.0
	_acc[1] = (float64(_accCounts[1])*_accelScale - _accB[1])*1000.0
	_acc[2] = (float64(_accCounts[2])*_accelScale - _accB[2])*1000.0

	_gyro[0] = float64(_gyroCounts[0]) *_gyroScale - _gyroB[0]
	_gyro[1] = float64(_gyroCounts[1]) *_gyroScale - _gyroB[1]
	_gyro[2] = float64(_gyroCounts[2]) *_gyroScale - _gyroB[2]
	/*
   	_acc[0] = (float64(tX[0]*_accCounts[0] + tX[1]*_accCounts[1] + tX[2]*_accCounts[2]) * _accelScale - _accB[0]) * _accS[0]
	_acc[1] = (float64(tY[0]*_accCounts[0] + tY[1]*_accCounts[1] + tY[2]*_accCounts[2]) * _accelScale - _accB[1] ) * _accS[1];
	_acc[2] = (float64(tZ[0]*_accCounts[0] + tZ[1]*_accCounts[1] + tZ[2]*_accCounts[2]) * _accelScale - _accB[2]) * _accS[2];

	_gyro[0] = (float64(tX[0]*_gyroCounts[0] + tX[1]*_gyroCounts[1] + tX[2]*_gyroCounts[2]) * _gyroScale) - _gyroB[0];
	_gyro[1] = (float64(tY[0]*_gyroCounts[0] + tY[1]*_gyroCounts[1] + tY[2]*_gyroCounts[2]) * _gyroScale) - _gyroB[1];
	_gyro[2] = (float64(tZ[0]*_gyroCounts[0] + tZ[1]*_gyroCounts[1] + tZ[2]*_gyroCounts[2]) * _gyroScale) - _gyroB[2];
	*/

	/*
	//_tcounts = (((int16_t)_buffer[0]) << 8) | _buffer[1];
	//_t = ((((double) _tcounts) - _tempOffset)/_tempScale) + _tempOffset;
	tb :=[2]byte{_buffer[0],_buffer[1]}
	_tcounts := BytesToInt16( tb[:])
	_t :=  float64(  _tcounts)/ 333.87 + 25;*/

	return nil
}

func (icm *Dev)  calibrateAccelx() error {
	// set the range, bandwidth, and srd
	_,err:=icm.setAccelRange(ACCEL_RANGE_2G)
	if ( err!=nil) {
		return err;
	}
	_accMin := [3]float64{};
	_accMax := [3]float64{};
	_accBD := [3]float64{};
	// take samples and find min / max
	_accBD[0] = 0;
	_accBD[1] = 0;
	_accBD[2] = 0;
	for  i:=0; i < _numSamples; i++ {
		icm.readSensor();
		_accBD[0] += (icm.getAccelX_mss()/_accS[0] + _accB[0])/(float64(_numSamples))
		_accBD[1] += (icm.getAccelY_mss()/_accS[1] + _accB[1])/(float64(_numSamples))
		_accBD[2] += (icm.getAccelZ_mss()/_accS[2] + _accB[2])/(float64(_numSamples))
		time.Delayms(20);
	}
	if (_accBD[0] > 9.0) {
		_accMax[0] =  _accBD[0];
	}
	if (_accBD[1] > 9.0) {
		_accMax[1] = _accBD[1];
	}
	if (_accBD[2] > 9.0) {
		_accMax[2] = _accBD[2];
	}
	if (_accBD[0] < -9.0) {
		_accMin[0] = _accBD[0];
	}
	if (_accBD[1] < -9.0) {
		_accMin[1] = _accBD[1];
	}
	if (_accBD[2] < -9.0) {
		_accMin[2] = _accBD[2];
	}
	// set the range, bandwidth, and srd back to what they were
	_,err=icm.setAccelRange(_accelRange)
	if ( err !=nil ) {
		return err//-4;
	}
	return nil;
}

func (icm *Dev)  calibrateAccel() error {
	// set the range, bandwidth, and srd
	_,err:=icm.setAccelRange(ACCEL_RANGE_2G)
	if ( err!=nil) {
		return err;
	}
	//_accMin := [3]float64{};
	//_accMax := [3]float64{};
	_accBD := [3]float64{};
	// take samples and find min / max
	_accBD[0] = 0;
	_accBD[1] = 0;
	_accBD[2] = 0;
	for  i:=0; i < _numSamples; i++ {
		icm.readSensor();
		_accBD[0] += icm.getAccelX_mss()//(icm.getAccelX_mss()/_accS[0] + _accB[0])/(float64(_numSamples))
		_accBD[1] += icm.getAccelY_mss()//(icm.getAccelY_mss()/_accS[1] + _accB[1])/(float64(_numSamples))
		_accBD[2] += icm.getAccelZ_mss()//(icm.getAccelZ_mss()/_accS[2] + _accB[2])/(float64(_numSamples))
		time.Delayms(20);
	}

	_accB[0] = _accBD[0]*_accelScale/float64(_numSamples)
	_accB[1] = _accBD[1]*_accelScale/float64(_numSamples)
	_accB[2] = _accBD[2]*_accelScale/float64(_numSamples)

	if (_accB[0] > 0.8) {
		_accB[0] -=  -1.0;
	}
	if (_accB[0] < -0.8) {
		_accB[0] +=  -1.0;
	}
	if (_accB[1] > 0.8) {
		_accB[1] -=  -1.0;
	}
	if (_accBD[1] < -0.8) {
		_accB[1]  +=  -1.0;
	}
	if (_accB[2] > 0.8) {
		_accB[2] -=  -1.0;
	}
	if (_accBD[2] < -0.8) {
		_accB[2]  +=  -1.0;
	}
	// set the range, bandwidth, and srd back to what they were
	_,err=icm.setAccelRange(_accelRange)
	if ( err !=nil ) {
		return err//-4;
	}
	return nil;
}
func  (icm *Dev)  calibrateGyro2() error{
	// set the range, bandwidth, and srd
	_,err:= icm.setGyroRange(GYRO_RANGE_250DPS)
	if (err!=nil) {
		return err;
	}
	// take samples and find bias
	_gyroBD := [3]float64{};
	_gyroBD[0] = 0;
	_gyroBD[1] = 0;
	_gyroBD[2] = 0;
	for  i:=0; i < _numSamples; i++  {
		icm.readSensor();
		_gyroBD[0] += icm.getGyroX_rads()// ( + _gyroB[0])/(float64(_numSamples));
		_gyroBD[1] += icm.getGyroY_rads() //(icm.getGyroY_rads() + _gyroB[1])/(float64(_numSamples));
		_gyroBD[2] += icm.getGyroZ_rads() //(icm.getGyroZ_rads() + _gyroB[2])/(float64(_numSamples));
		time.Delayms(30);
	}
	_gyroB[0] = _gyroBD[0]*_gyroScale/float64(_numSamples);
	_gyroB[1] = _gyroBD[1]*_gyroScale/float64(_numSamples);
	_gyroB[2] = _gyroBD[2]*_gyroScale/float64(_numSamples);
	// set the range, bandwidth, and srd back to what they were
	fmt.Println(_gyroB)
	_,err = icm.setGyroRange(_gyroRange)
	if ( err !=nil) {
		return err;//-4;
	}
	return nil;
}

func  (icm *Dev)  calibrateGyro() error{
// set the range, bandwidth, and srd
	_,err:= icm.setGyroRange(GYRO_RANGE_250DPS)
	if (err!=nil) {
		return err;
	}
	// take samples and find bias
	_gyroBD := [3]float64{};
	_gyroBD[0] = 0;
	_gyroBD[1] = 0;
	_gyroBD[2] = 0;
	for  i:=0; i < _numSamples; i++  {
		icm.readSensor();
		_gyroBD[0] += (icm.getGyroX_rads() + _gyroB[0])/(float64(_numSamples));
		_gyroBD[1] += (icm.getGyroY_rads() + _gyroB[1])/(float64(_numSamples));
		_gyroBD[2] += (icm.getGyroZ_rads() + _gyroB[2])/(float64(_numSamples));
		time.Delayms(30);
	}
	_gyroB[0] = _gyroBD[0];
	_gyroB[1] = _gyroBD[1];
	_gyroB[2] = _gyroBD[2];
	// set the range, bandwidth, and srd back to what they were
	//fmt.Println(_gyroB)
	_,err = icm.setGyroRange(_gyroRange)
	if ( err !=nil) {
		return err;//-4;
	}
	return nil;
}

func (icm *Dev) setAccelRange(rangev AccelRange ) (int,error){
	switch(rangev) {
		case ACCEL_RANGE_2G: {
			// setting the accel range to 2G
			err :=icm.m.WriteUint8(ACCEL_CONFIG0,ACCEL_FS_SEL_2G)
			if( err !=nil) {
				return -1,err;
			}
			//_accelScale = G * 2.0/32767.5; // setting the accel scale to 2G
			_accelScale =  2.0/32768.0;
			break;
			}
		case ACCEL_RANGE_4G: {
			// setting the accel range to 4G
			err :=icm.m.WriteUint8(ACCEL_CONFIG0,ACCEL_FS_SEL_4G)
			if( err !=nil) {
				return -1,err;
			}
			//_accelScale = G * 4.0/32767.5; // setting the accel scale to 4G
			_accelScale =  4.0/32768.0;
			break;
		}
		case ACCEL_RANGE_8G: {
			// setting the accel range to 8G
			err :=icm.m.WriteUint8(ACCEL_CONFIG0,ACCEL_FS_SEL_8G)
			if( err !=nil) {
				return -1,err;
			}
			//_accelScale = G * 8.0/32767.5; // setting the accel scale to 8G
			_accelScale =  8.0/32768.0;
			break;
		}
		case ACCEL_RANGE_16G: {
			// setting the accel range to 16G
			err :=icm.m.WriteUint8(ACCEL_CONFIG0,ACCEL_FS_SEL_16G)
			if( err !=nil) {
				return -1,err;
			}
			//_accelScale = G * 16.0/32767.5; // setting the accel scale to 16G
			_accelScale =  16.0/32768.0;
			break;
		}
	}
	_accelRange = rangev;
	return 1,nil
}

func  (icm *Dev) setGyroRange(rangev GyroRange ) (int,error){
	switch(rangev) {
		case GYRO_RANGE_15_625DPS: {
			// setting the gyro range to 15.625DPS
			err:=icm.m.WriteUint8(GYRO_CONFIG0,GYRO_FS_SEL_15_625DPS)
			if(err !=nil) {
			return -1,err;
			}
			_gyroScale = 15.625/32767.5 * _d2r; // setting the gyro scale to 15.625DPS
			//_gyroScale = 15.625/32768.0;
			break;
		}
		case GYRO_RANGE_31_25DPS: {
			// setting the gyro range to 31.25DPS
			err :=icm.m.WriteUint8(GYRO_CONFIG0,GYRO_FS_SEL_31_25DPS)
			if(err !=nil) {
				return -1,err;
			}
			_gyroScale = 31.25/32767.5 * _d2r; // setting the gyro scale to 31.25DPS
			//_gyroScale = 31.25/32768.0;
			break;
		}
		case GYRO_RANGE_62_5DPS: {
			// setting the gyro range to 62.5DPS
			err :=icm.m.WriteUint8(GYRO_CONFIG0,GYRO_FS_SEL_62_5DPS)
			if(err !=nil) {
				return -1,err;
			}
			_gyroScale = 62.5/32767.5 * _d2r; // setting the gyro scale to 62.5DPS
			//_gyroScale = 62.5/32768.0;
			break;
		}
		case GYRO_RANGE_125DPS: {
			// setting the gyro range to 125DPS
			err :=icm.m.WriteUint8(GYRO_CONFIG0,GYRO_FS_SEL_125DPS)
			if(err !=nil) {
				return -1,err;
			}
			_gyroScale = 125.0/32767.5 * _d2r; // setting the gyro scale to 125DPS
			//_gyroScale = 125.0/32768.0;
			break;
		}
		case GYRO_RANGE_250DPS: {
			// setting the gyro range to 250DPS
			err :=icm.m.WriteUint8(GYRO_CONFIG0,GYRO_FS_SEL_250DPS)
			if(err !=nil) {
				return -1,err;
			}
			_gyroScale = 250.0/32767.5 * _d2r; // setting the gyro scale to 250DPS
			//_gyroScale = 250.0/32768.0;
			break;
		}
		case GYRO_RANGE_500DPS: {
			// setting the gyro range to 500DPS
			err :=icm.m.WriteUint8(GYRO_CONFIG0,GYRO_FS_SEL_500DPS)
			if(err !=nil) {
				return -1,err;
			}
			_gyroScale = 500.0/32767.5 * _d2r; // setting the gyro scale to 500DPS
			//_gyroScale = 500.0/32768.0;
			break;
		}
		case GYRO_RANGE_1000DPS: {
			// setting the gyro range to 1000DPS
			err :=icm.m.WriteUint8(GYRO_CONFIG0,GYRO_FS_SEL_1000DPS)
			if(err !=nil) {
				return -1,err;
			}
			_gyroScale = 1000.0/32767.5 * _d2r; // setting the gyro scale to 1000DPS
			//_gyroScale = 1000.0/32768.0;
			break;
		}
		case GYRO_RANGE_2000DPS: {
			// setting the gyro range to 2000DPS
			err :=icm.m.WriteUint8(GYRO_CONFIG0,GYRO_FS_SEL_2000DPS)
			if(err !=nil) {
				return -1,err;
			}
			_gyroScale = 2000.0/32767.5 * _d2r; // setting the gyro scale to 2000DPS
			//_gyroScale = 2000.0/32768.0;
			break;
		}
	}
	_gyroRange  = rangev;
	return 1,nil;
}
// Dev is a handle to the ina219 sensor.
type Dev struct {
	m mmr.Dev8
	mu         sync.Mutex
/*
	currentLSB physic.ElectricCurrent
	powerLSB   physic.Power*/
	// calibration variables
 }
/*
func (ina226 *Dev) ReadBusVoltage() (float64, error) {
	voltage, err := ina226.m.ReadUint16(BUSVOLTAGE_REG)
	if err != nil {
		return 0, err
	}
	return float64(voltage) * 1.25, nil
}*/
func BytesToInt16(buf []byte) int16 {
	return int16(binary.BigEndian.Uint16(buf))
}
var (
 	q = [4]float64{1.0, 0.0, 0.0, 0.0}
	PI = 3.141592653589793238462643383279502884
	GyroMeasError = PI * (40.0  / 180.0 )
	GyroMeasDrift = PI * (0.0  / 180.0);
	beta = math.Sqrt(3.0 / 4.0) * GyroMeasError
	zeta = math.Sqrt(3.0 / 4.0) * GyroMeasDrift
)

func  (icm *Dev)  PrintZT()  {
	a12 :=   2.0 * (q[1] * q[2] + q[0] * q[3]);
	a22 :=   q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
	a31 :=   2.0 * (q[0] * q[1] + q[2] * q[3]);
	a32 :=   2.0 * (q[1] * q[3] - q[0] * q[2]);
	a33 :=   q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
	pitch := -math.Asin(a32);
	roll  := math.Atan2(a31, a33);
	yaw   := math.Atan2(a12, a22);
	pitch *= 180.0 / PI;
	yaw   *= 180.0 / PI;
	//yaw   += 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
	if  yaw < 0{
		yaw   += 360.0; // Ensure yaw stays between 0 and 360
	}
	roll  *= 180.0  / PI;
	fmt.Printf("yaw:%f ,pitch:%f ,roll:%f \r\n",yaw,pitch,roll)
	//fmt.Printf("q1:%f ,q2:%f ,q3:%f \r\n",q[1],q[2],q[3])
}
func  (icm *Dev)  MadgwickQuaternionUpdate( ax float64 ,  ay float64,  az float64,  gyrox float64,  gyroy float64,  gyroz float64, deltat float64){
	q1 := q[0]
	q2 := q[1]
	q3 := q[2]
	q4 := q[3];         // short name local variable for readability
	var norm float64;                                               // vector norm
	var f1 float64;
	var f2 float64;
	var f3 float64;     // objetive funcyion elements

	var J_11or24 float64
	var J_12or23 float64
	var J_13or22 float64
	var J_14or21 float64
	var J_32 float64
	var J_33 float64 // objective function Jacobian elements

	var qDot1 float64
	var qDot2 float64
	var qDot3 float64
	var qDot4 float64

	var hatDot1 float64
	var hatDot2 float64
	var hatDot3 float64
	var hatDot4 float64

	var gerrx float64
	var gerry float64
	var gerrz float64
	var gbiasx float64
	var gbiasy float64
	var gbiasz float64        // gyro bias error

	// Auxiliary variables to avoid repeated arithmetic
	 _halfq1 := 0.5 * q1;
	 _halfq2 := 0.5 * q2;
	 _halfq3 := 0.5 * q3;
	 _halfq4 := 0.5 * q4;
	 _2q1 := 2.0 * q1;
	 _2q2 := 2.0 * q2;
	 _2q3 := 2.0 * q3;
	 _2q4 := 2.0 * q4;
	 //_2q1q3 := 2.0 * q1 * q3;
	 //_2q3q4 := 2.0 * q3 * q4;

	// Normalise accelerometer measurement
	norm = math.Sqrt(ax * ax + ay * ay + az * az);
	if (norm == 0.0){
		return
	}; // handle NaN
	norm = 1.0/norm;
	ax *= norm;
	ay *= norm;
	az *= norm;

	// Compute the objective function and Jacobian
	f1 = _2q2 * q4 - _2q1 * q3 - ax;
	f2 = _2q1 * q2 + _2q3 * q4 - ay;
	f3 = 1.0 - _2q2 * q2 - _2q3 * q3 - az;
	J_11or24 = _2q3;
	J_12or23 = _2q4;
	J_13or22 = _2q1;
	J_14or21 = _2q2;
	J_32 = 2.0 * J_14or21;
	J_33 = 2.0 * J_11or24;

	// Compute the gradient (matrix multiplication)
	hatDot1 = J_14or21 * f2 - J_11or24 * f1;
	hatDot2 = J_12or23 * f1 + J_13or22 * f2 - J_32 * f3;
	hatDot3 = J_12or23 * f2 - J_33 *f3 - J_13or22 * f1;
	hatDot4 = J_14or21 * f1 + J_11or24 * f2;

	// Normalize the gradient
	norm = math.Sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 * hatDot4);
	hatDot1 /= norm;
	hatDot2 /= norm;
	hatDot3 /= norm;
	hatDot4 /= norm;

	// Compute estimated gyroscope biases
	gerrx = _2q1 * hatDot2 - _2q2 * hatDot1 - _2q3 * hatDot4 + _2q4 * hatDot3;
	gerry = _2q1 * hatDot3 + _2q2 * hatDot4 - _2q3 * hatDot1 - _2q4 * hatDot2;
	gerrz = _2q1 * hatDot4 - _2q2 * hatDot3 + _2q3 * hatDot2 - _2q4 * hatDot1;

	// Compute and remove gyroscope biases
	gbiasx += gerrx *  ( deltat) * zeta;
	gbiasy += gerry *  ( deltat) * zeta;
	gbiasz += gerrz *  ( deltat) * zeta;
	gyrox -= gbiasx;
	gyroy -= gbiasy;
	gyroz -= gbiasz;

	// Compute the quaternion derivative
	qDot1 = -_halfq2 * gyrox - _halfq3 * gyroy - _halfq4 * gyroz;
	qDot2 =  _halfq1 * gyrox + _halfq3 * gyroz - _halfq4 * gyroy;
	qDot3 =  _halfq1 * gyroy - _halfq2 * gyroz + _halfq4 * gyrox;
	qDot4 =  _halfq1 * gyroz + _halfq2 * gyroy - _halfq3 * gyrox;

	// Compute then integrate estimated quaternion derivative
	q1 += (qDot1 -(beta * hatDot1)) * ( deltat);
	q2 += (qDot2 -(beta * hatDot2)) *  ( deltat);
	q3 += (qDot3 -(beta * hatDot3)) *  ( deltat);
	q4 += (qDot4 -(beta * hatDot4)) *  ( deltat);

	// Normalize the quaternion
	norm = math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
	norm = 1.0/norm;
	q[0] = q1 * norm;
	q[1] = q2 * norm;
	q[2] = q3 * norm;
	q[3] = q4 * norm;
}
/*
//g表陀螺仪，a表加计
func IMUupdate(gx float64,gy float64,gz float64,ax float64,ay float64,az float64) (x D3){
	q0temp,q1temp,q2temp ,q3temp :=0.0,0.0,0.0,0.0  //四元数暂存变量，求解微分方程时要用
	norm:=0.0; //矢量的模或四元数的范数
	vx, vy, vz:=0.0,0.0,0.0;//当前姿态计算得来的重力在三轴上的分量
	ex, ey, ez:=0.0,0.0,0.0;//当前加计测得的重力加速度在三轴上的分量
	//与用当前姿态计算得来的重力在三轴上的分量的误差
	// 先把这些用得到的值算好
	 q0q0 := q0*q0;
	 q0q1 := q0*q1;
	 q0q2 := q0*q2;
	 q1q1 := q1*q1;
	 q1q3 := q1*q3;
	 q2q2 := q2*q2;
	 q2q3 := q2*q3;
	 q3q3 := q3*q3;
	//加计处于自由落体状态时不进行姿态解算，因为会产生分母无穷大的情况
	if (ax*ay*az==0) {
		return D3{0, 0, 0}
	}
	norm = math.Sqrt(ax*ax + ay*ay + az*az);//单位化加速度计，
	ax = ax /norm;// 这样变更了量程也不需要修改KP参数，因为这里归一化了
	ay = ay / norm;
	az = az / norm;
	//用当前姿态计算出重力在三个轴上的分量，
	//参考坐标n系转化到载体坐标b系的用四元数表示的方向余弦矩阵第三列即是（博文一中有提到）
	vx = 2*(q1q3 - q0q2);
	vy = 2*(q0q1 + q2q3);
	vz = q0q0 - q1q1 - q2q2 + q3q3 ;
	//计算测得的重力与计算得重力间的误差，向量外积可以表示这一误差
	//原因我理解是因为两个向量是单位向量且sin0等于0
	//不过要是夹角是180度呢~这个还没理解
	ex = (ay*vz - az*vy) ;
	ey = (az*vx - ax*vz) ;
	ez = (ax*vy - ay*vx) ;

	exInt = exInt + ex * Ki;                                           //对误差进行积分
	eyInt = eyInt + ey * Ki;
	ezInt = ezInt + ez * Ki;
	// adjusted gyroscope measurements
	gx = gx + Kp*ex + exInt;  //将误差PI后补偿到陀螺仪，即补偿零点漂移
	gy = gy + Kp*ey + eyInt;
	gz = gz + Kp*ez + ezInt;    //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减
	//下面进行姿态的更新，也就是四元数微分方程的求解
	q0temp=q0;//暂存当前值用于计算
	q1temp=q1;//网上传的这份算法大多没有注意这个问题，在此更正
	q2temp=q2;
	q3temp=q3;
	//采用一阶毕卡解法，相关知识可参见《惯性器件与惯性导航系统》P212
	q0 = q0temp + (-q1temp*gx - q2temp*gy -q3temp*gz)*halfT;
	q1 = q1temp + (q0temp*gx + q2temp*gz -q3temp*gy)*halfT;
	q2 = q2temp + (q0temp*gy - q1temp*gz +q3temp*gx)*halfT;
	q3 = q3temp + (q0temp*gz + q1temp*gy -q2temp*gx)*halfT;
	//单位化四元数在空间旋转时不会拉伸，仅有旋转角度，这类似线性代数里的正交变换
	norm = math.Sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
	q0 = q0 / norm;
	q1 = q1 / norm;
	q2 = q2 / norm;
	q3 = q3 / norm;
	//四元数到欧拉角的转换，公式推导见博文一
	//其中YAW航向角由于加速度计对其没有修正作用，因此此处直接用陀螺仪积分代替
	d:=D3{}
	d.Z = math.Atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) //  GYRO_I.Z; // yaw
	d.Y = math.Asin(-2 * q1 * q3 + 2 * q0* q2) //*57.3; // pitch
	d.X = math.Atan2(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1 - 2 * q2* q2 + 1)//* 57.3; // roll
	return d
}*/

// Sense reads the power values from the ina219 sensor.
func (d *Dev) Sense() (MotionMonitor, error) {
	d.mu.Lock()
	defer d.mu.Unlock()
	var _buffer [14]byte
	var pm MotionMonitor
	err :=d.m.ReadStruct(TEMP_OUT, &_buffer)
	if ( err !=nil) {
		return pm,err;
	}
	//fmt.Println(_buffer)

	a1:=[2]byte{_buffer[2],_buffer[3] }
	a2:=[2]byte{_buffer[4],_buffer[5] }
	a3:=[2]byte{_buffer[6],_buffer[6] }

	g1:=[2]byte{_buffer[8],_buffer[9] }
	g2:=[2]byte{_buffer[10],_buffer[11] }
	g3:=[2]byte{_buffer[12],_buffer[13] }

	_accCounts[0] = BytesToInt16(a1[:] ) //(((int16_t)_buffer[2]) << 8) | _buffer[3];
	_accCounts[1] = BytesToInt16(a2[:] ) //(((int16_t)_buffer[4]) << 8) | _buffer[5];
	_accCounts[2] = BytesToInt16(a3[:] ) //(((int16_t)_buffer[6]) << 8) | _buffer[7];

	_gyroCounts[0] = BytesToInt16(g1[:] )
	_gyroCounts[1] = BytesToInt16(g2[:] )
	_gyroCounts[2] = BytesToInt16(g3[:] )

	 /*
	_acc[0] = (float64(tX[0]*_accCounts[0] + tX[1]*_accCounts[1] + tX[2]*_accCounts[2]) * _accelScale - _accB[0]) * _accS[0]
	_acc[1] = (float64(tY[0]*_accCounts[0] + tY[1]*_accCounts[1] + tY[2]*_accCounts[2]) * _accelScale - _accB[1]) * _accS[1];
	_acc[2] = (float64(tZ[0]*_accCounts[0] + tZ[1]*_accCounts[1] + tZ[2]*_accCounts[2]) * _accelScale - _accB[2]) * _accS[2];
	*/
	_acc[0] = (float64(_accCounts[0])*_accelScale - _accB[0])*1000.0
	_acc[1] = (float64(_accCounts[1])*_accelScale - _accB[1])*1000.0
	_acc[2] = (float64(_accCounts[2])*_accelScale - _accB[2])*1000.0

	/*_gyro[0] = (float64(tX[0]*_gyroCounts[0] + tX[1]*_gyroCounts[1] + tX[2]*_gyroCounts[2]) * _gyroScale) - _gyroB[0];
	_gyro[1] = (float64(tY[0]*_gyroCounts[0] + tY[1]*_gyroCounts[1] + tY[2]*_gyroCounts[2]) * _gyroScale) - _gyroB[1];
	_gyro[2] = (float64(tZ[0]*_gyroCounts[0] + tZ[1]*_gyroCounts[1] + tZ[2]*_gyroCounts[2]) * _gyroScale) - _gyroB[2];
	*/
	/*
	_gyro[0] = float64(tX[0]*_gyroCounts[0]) * _gyroScale - _gyroB[0]// (float64(tX[0]*_gyroCounts[0] + tX[1]*_gyroCounts[1] + tX[2]*_gyroCounts[2]) * _gyroScale) - _gyroB[0];
	_gyro[1] = float64(tY[0]*_gyroCounts[1]) * _gyroScale - _gyroB[1]// (float64(tY[0]*_gyroCounts[0] + tY[1]*_gyroCounts[1] + tY[2]*_gyroCounts[2]) * _gyroScale) - _gyroB[1];
	_gyro[2] = float64(tZ[0]*_gyroCounts[2]) * _gyroScale - _gyroB[2] //(float64(tZ[0]*_gyroCounts[0] + tZ[1]*_gyroCounts[1] + tZ[2]*_gyroCounts[2]) * _gyroScale) - _gyroB[2];
	*/
	_gyro[0] = float64(_gyroCounts[0]) *_gyroScale  - _gyroB[0]
	_gyro[1] = float64(_gyroCounts[1]) *_gyroScale  - _gyroB[1]
	_gyro[2] = float64(_gyroCounts[2]) *_gyroScale  - _gyroB[2]


	tb :=[2]byte{_buffer[0],_buffer[1]}
	_tcounts := BytesToInt16( tb[:])
	_t :=  float64(  _tcounts)/ 333.87 + 25;

	pm.Accele = D3{_acc[0],_acc[1],_acc[2]}
	pm.Gyro = D3{_gyro[0],_gyro[1],_gyro[2]}
	pm.Temperature =_t;

	return pm, nil
}
func (p D3) String() string {
	return fmt.Sprintf("(%.3f,%.3f,%.3f)",p.X,p.Y,p.Z)
}
// String returns a PowerMonitor as string
func (p MotionMonitor) String() string {
	return fmt.Sprintf("Temperature:%0.2f Magnetic:%s, Gyro:%s, Accele:%s ",p.Temperature, p.Magnetic, p.Gyro, p.Accele)
}
/* returns the accelerometer measurement in the x direction, m/s/s */
func (d *Dev) getAccelX_mss() float64 {
	return _acc[0];
}

/* returns the accelerometer measurement in the y direction, m/s/s */
func (d *Dev) getAccelY_mss() float64{
	return _acc[1];
}

/* returns the accelerometer measurement in the z direction, m/s/s */
func (d *Dev) getAccelZ_mss() float64{
	return _acc[2];
}
/* returns the gyroscope measurement in the x direction, rad/s */
func (d *Dev) getGyroX_rads() float64 {
	return _gyro[0];
}

/* returns the gyroscope measurement in the y direction, rad/s */
func (d *Dev) getGyroY_rads() float64{
	return _gyro[1];
}

/* returns the gyroscope measurement in the z direction, rad/s */
func (d *Dev) getGyroZ_rads() float64 {
	return _gyro[2];
}

/* returns the gyroscope measurement in the x direction, rad/s */
func (d *Dev)   getGyroX_dps() float64 {
	return _gyro[0]*_r2d;
}

/* returns the gyroscope measurement in the y direction, rad/s */
func (d *Dev)  getGyroY_dps()float64 {
	return _gyro[1]*_r2d;
}

/* returns the gyroscope measurement in the z direction, rad/s */
func (d *Dev) getGyroZ_dps()float64 {
	return _gyro[2]*_r2d;
}


func  (d *Dev)setFilters( gyroFilters bool,  accFilters bool)  error{
	err := d.m.WriteUint8(BANK_SEL, BANK1)
	if (err!=nil) {
	return err;//-1
	}
	if (gyroFilters == true) {
		err := d.m.WriteUint8(GYRO_CONFIG_STATIC2, GYRO_NF_ENABLE | GYRO_AAF_ENABLE)
		if (err!=nil) {
			return err;//-2
		}
	}else {
		err := d.m.WriteUint8(GYRO_CONFIG_STATIC2, GYRO_NF_DISABLE | GYRO_AAF_DISABLE)
		if (err!=nil) {
			return err;// -3
		}
	}
	err = d.m.WriteUint8(BANK_SEL, BANK2)
	if (err!=nil) {
		return err;// -4
	}
	if (accFilters == true) {
		err = d.m.WriteUint8(ACCEL_CONFIG_STATIC2, ACCEL_AAF_ENABLE)
		if (err!=nil) {
			return err;// -5
		}
	}else {
		err = d.m.WriteUint8(ACCEL_CONFIG_STATIC2, ACCEL_AAF_DISABLE)
		if (err!=nil) {
			return err;// -6
		}
	}
	err = d.m.WriteUint8(BANK_SEL, BANK0)
	if (err!=nil) {
		return err;// -7
	}
	return nil;
}

/* returns the die temperature, C
func (d *Dev) getTemperature_C() float64 {
	return _t;
}*/

var (
	errReadShunt                 = errors.New("failed to read shunt voltage")
	errReadBus                   = errors.New("failed to read bus voltage")
	errReadPower                 = errors.New("failed to read power")
	errReadCurrent               = errors.New("failed to read current")
	errAddressOutOfRange         = errors.New("i2c address out of range")
	errSenseResistorValueInvalid = errors.New("sense resistor value cannot be negative or zero")
	errMaxCurrentInvalid         = errors.New("max current cannot be negative or zero")
	errRegisterOverflow          = errors.New("bus voltage register overflow")
	errWritingToConfigRegister   = errors.New("failed to write to configuration register")
	errCalibrationOverflow       = errors.New("calibration would exceed maximum scaling")
)
